/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          : freertos.c
 * Description        : Code for freertos applications
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "control.h"
#include "delay.h"
#include "encoder.h"
#include "motor.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "myiic.h"
#include "draw_api.h"
#include "pid.h"

#include "tim.h"
#include "queue.h"
#include "semphr.h"

#include "stdio.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for Task_Main */
osThreadId_t Task_MainHandle;
const osThreadAttr_t Task_Main_attributes = {
  .name = "Task_Main",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Task_PidCtrl */
osThreadId_t Task_PidCtrlHandle;
const osThreadAttr_t Task_PidCtrl_attributes = {
  .name = "Task_PidCtrl",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityAboveNormal,
};
/* Definitions for Task_Info */
osThreadId_t Task_InfoHandle;
const osThreadAttr_t Task_Info_attributes = {
  .name = "Task_Info",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow,
};
/* Definitions for Queue_Host */
osMessageQueueId_t Queue_HostHandle;
const osMessageQueueAttr_t Queue_Host_attributes = {
  .name = "Queue_Host"
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void AppTask_Main(void *argument);
void AppTask_PidCtrl(void *argument);
void AppTask_Info(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* creation of Queue_Host */
  Queue_HostHandle = osMessageQueueNew (10, 5, &Queue_Host_attributes);

  /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of Task_Main */
  Task_MainHandle = osThreadNew(AppTask_Main, NULL, &Task_Main_attributes);

  /* creation of Task_PidCtrl */
  Task_PidCtrlHandle = osThreadNew(AppTask_PidCtrl, NULL, &Task_PidCtrl_attributes);

  /* creation of Task_Info */
  Task_InfoHandle = osThreadNew(AppTask_Info, NULL, &Task_Info_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
    /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_AppTask_Main */
/**
 * @brief  Function implementing the Task_Main thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_AppTask_Main */
void AppTask_Main(void *argument)
{
  /* USER CODE BEGIN AppTask_Main */
    Host_CmdStructDef cmd = {0};


    /* Infinite loop */
    for (;;) {
        BaseType_t result = xQueueReceive(Queue_HostHandle, &cmd, 0);
        if (result == pdTRUE) {
            host_cmdDeal(cmd);
        }

        // taskENTER_CRITICAL();
        printf("%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
               pitch,
               roll,
               yaw,
               pidInfo_Speed.target,
               pidInfo_Speed.curActualL,
               motor_curPwmL,
               pidInfo_Speed.target,
               pidInfo_Speed.curActualR,
               motor_curPwmR);
        // printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d\r\n",
        //        pitch,
        //        roll,
        //        yaw,
        //        pidInfo_SpeedL.target,
        //        pidInfo_SpeedL.curActual,
        //        motor_curPwmL,
        //        pidInfo_SpeedR.target,
        //        pidInfo_SpeedR.curActual,
        //        motor_curPwmR,
        //        gyrox,
        //        gyroy,
        //        gyroz);
        // printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d\r\n",
        //        pitch,
        //        roll,
        //        yaw,
        //        pidInfo_Speed.target,
        //        pidInfo_Speed.curActualL,
        //        motor_curPwmL,
        //        pidInfo_Speed.target,
        //        pidInfo_Speed.curActualR,
        //        motor_curPwmR,
        //        gyrox,
        //        gyroy,
        //        gyroz);
        // taskEXIT_CRITICAL();


        vTaskDelay(pdMS_TO_TICKS(40));
    }
  /* USER CODE END AppTask_Main */
}

/* USER CODE BEGIN Header_AppTask_PidCtrl */
/**
 * @brief Function implementing the Task_PidCtrl thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_AppTask_PidCtrl */
void AppTask_PidCtrl(void *argument)
{
  /* USER CODE BEGIN AppTask_PidCtrl */
    TickType_t previousWakeTIme = xTaskGetTickCount();
    /* Infinite loop */
    for (;;) {
        pidInfo_Speed.preActualL = pidInfo_Speed.curActualL;
        pidInfo_Speed.preActualR = pidInfo_Speed.curActualR;

        pidInfo_Speed.curActualL = Encoder_ReadData(&htim2);
        pidInfo_Speed.curActualR = -Encoder_ReadData(&htim4);

        if (roll > ANGLE_MAX || roll < ANGLE_MIN) {
            run_flag = 0;
        }

        if (run_flag != 0) {
            float speedOut = 0;
            float standOut = 0;
            float turnOut  = 0;

            speedOut = pid_calSpeed(&pidInfo_Speed);
            standOut = pid_calStand(target_angle + speedOut, roll, gyrox - 23);
            turnOut  = pid_calTurn(target_turn, gyroz - 16);

            motor_curPwmL = standOut + turnOut;
            motor_curPwmR = standOut - turnOut;

            if (motor_curPwmL > PWM_MAX) {
                motor_curPwmL = PWM_MAX;
            }
            if (motor_curPwmL < PWM_MIN) {
                motor_curPwmL = PWM_MIN;
            }
            if (motor_curPwmR > PWM_MAX) {
                motor_curPwmR = PWM_MAX;
            }
            if (motor_curPwmR < PWM_MIN) {
                motor_curPwmR = PWM_MIN;
            }

            motor_driveL(motor_curPwmL);
            motor_driveR(motor_curPwmR);
        }
        else {
            pidInfo_Speed.error0   = 0;
            pidInfo_Speed.error1   = 0;
            pidInfo_Speed.errorInt = 0;

            motor_curPwmL = 0;
            motor_curPwmR = 0;

            target_turn = 0;

            motor_driveL(0);
            motor_driveR(0);
        }

        vTaskDelayUntil(&previousWakeTIme, pdMS_TO_TICKS(40));
    }
  /* USER CODE END AppTask_PidCtrl */
}

/* USER CODE BEGIN Header_AppTask_Info */
/**
 * @brief Function implementing the Task_Info thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_AppTask_Info */
void AppTask_Info(void *argument)
{
  /* USER CODE BEGIN AppTask_Info */
    char tempstr[16] = {0};

    taskENTER_CRITICAL();
    SetFontSize(1);

    TaskHandle_t taskHandle = Task_MainHandle;
    UBaseType_t  hwm        = uxTaskGetStackHighWaterMark(taskHandle);
    sprintf(tempstr, "Main %3lu", hwm);
    DrawString(0, 8 * 0, tempstr);

    taskHandle = Task_PidCtrlHandle;
    hwm        = uxTaskGetStackHighWaterMark(taskHandle);
    sprintf(tempstr, "Pid %3lu", hwm);
    DrawString(0, 8 * 1, tempstr);

    // taskHandle = Task_InfoHandle;
    // hwm        = uxTaskGetStackHighWaterMark(taskHandle);
    // sprintf(tempstr, "Info %3lu", hwm);
    // DrawString(0, 8 * 2, tempstr);

    taskEXIT_CRITICAL();
    UpdateScreen();


    /* Infinite loop */
    for (;;) {
        break;
    }

    vTaskDelete(NULL);
  /* USER CODE END AppTask_Info */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

